#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
import math,random
from std_msgs.msg import Int8,Empty
from xarm_moveit_demo.srv import *
from geometry_msgs.msg import PoseStamped, Pose
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import PlanningScene, ObjectColor
from moveit_msgs.msg import Grasp,PlaceLocation,GripperTranslation, MoveItErrorCodes
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from ar_track_alvar_msgs.msg import AlvarMarkers
from tf.transformations import quaternion_from_euler,euler_from_quaternion
from copy import deepcopy
from xarm_vision.srv import *

GROUP_NAME_ARM = 'xarm'
GROUP_NAME_GRIPPER = 'gripper'
GRIPPER_FRAME = 'gripper_centor_link'
GRIPPER_OPEN = [0.68,0.68]
GRIPPER_GRASP = [0.25,0.25]
GRIPPER_CLOSED = [0.0,0.0]
GRIPPER_JOINT_NAMES = ['gripper_1_joint','gripper_2_joint']
GRIPPER_EFFORT = [1.0,1.0]
REFERENCE_FRAME = 'base_link'

class CallCompete:
    def __init__(self):
        # 初始化Python API 依赖的moveit_commanderC++系统
        moveit_commander.roscpp_initialize(sys.argv)
        # 初始化节点
        rospy.init_node('start_compete')

        # 初始化场景对象，用来在规划场景中添加或移除物体
        self.scene = PlanningSceneInterface()
        rospy.sleep(1)

        self.compete_client_ = rospy.ServiceProxy('/compete/pick_place', CallPickup)

        self.call_compete()

        rospy.spin()

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)

    def call_compete(self):
        for i in range(0,20):
            for i in [1,3]:
                print "Start pickup target: " +str(i)
                result = self.compete_client_.call(i)
                if result.success == True:
                    print "Pickup and place target  successfully!!!"
                else :
                    print "Failed to pickup and place the target!!!"
            # 删除规划场景里的物体
            for i in [1,3]:
                self.scene.remove_world_object(str(i))

        rospy.sleep(2)
        print "Done and exit !"
        sys.exit()



if __name__ == "__main__":
    CallCompete()
